// --------------------------------------------------------------------------------------------------------------------
// <copyright file="HeartbeatModelAgent.cs" company="Microsoft Corporation">
// The MIT License (MIT)
// 
// Copyright (c) 2014, Microsoft Corporation
// 
// Permission is hereby granted, free of charge, to any person obtaining a copy
//  of this software and associated documentation files (the "Software"), to deal
//  in the Software without restriction, including without limitation the rights
//  to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
//  copies of the Software, and to permit persons to whom the Software is
//  furnished to do so, subject to the following conditions:
// 
// The above copyright notice and this permission notice shall be included in
//  all copies or substantial portions of the Software.
// 
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
//  IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
//  FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
//  AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
//  LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
//  OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
//  THE SOFTWARE.
// </copyright>
// --------------------------------------------------------------------------------------------------------------------
namespace Microsoft.Robotics.Hardware.EV4.Visualization
{
    using System;
    using System.Runtime.Serialization;
    using Microsoft.Robotics.Hardware.Runtime.DAC;
    using Microsoft.Robotics.Runtime;
    using Microsoft.Robotics.Visualization;

    /// <summary>
    /// Maintains the model for the Heartbeat visualizer
    /// </summary>
    [DataContract]
    public sealed class HeartbeatModelAgent : Agent,
                                              ISubscriptionReceiver<BatteryMessage>,
                                              ISubscriptionReceiver<DifferentialDriveMessage>,
                                              ISubscriptionReceiver<FanMessage>,
                                              ISubscriptionReceiver<HumidityMessage>,
                                              ISubscriptionReceiver<IRProximityMessage>,
                                              ISubscriptionReceiver<JointMessage>,
                                              ISubscriptionReceiver<SonarMessage>,
                                              ISubscriptionReceiver<TemperatureMessage>,
                                              IControlReceiver<ViewInitializationMessage>
    {
        /// <summary>
        /// The default update interval, if one is not specified.
        /// </summary>
        private const int DefaultUpdateInterval = 50;

        /// <summary>
        /// The interval on which to issue data binding updates to the UI
        /// </summary>
        [DataMember(Name = "UpdateIntervalMilliseconds")]
        private int maxUpdateIntervalMilliseconds;

        /// <summary>
        /// The interval on which to issue data binding updates to the UI
        /// </summary>
        [DataMember(Name = "Producer")]
        private string producerName = default(string);

        /// <summary>
        /// The agent we are subscribing to.
        /// </summary>
        private IAgentHost producerAgent;

        /// <summary>
        /// The model maintained by this agent.
        /// </summary>
        private HeartbeatModel model;

        /// <summary>
        /// The timestamp of the last update we kept 
        /// </summary>
        private long lastTimestampInMilliseconds;

        /// <summary>
        /// The sequenceId of the last update we kept.
        /// </summary>
        private long lastSequenceId;

        /// <summary>
        /// Finds and subscribes to the publishing agent.
        /// </summary>
        /// <param name="locator">A locator to use.</param>
        public override void Initialize(AgentLocator locator)
        {
            base.Initialize(locator);
            this.producerAgent = locator.GetAgent(this.producerName);
            this.producerAgent.Subscribe<BatteryMessage>(this);
            this.producerAgent.Subscribe<DifferentialDriveMessage>(this);
            this.producerAgent.Subscribe<FanMessage>(this);
            this.producerAgent.Subscribe<HumidityMessage>(this);
            this.producerAgent.Subscribe<IRProximityMessage>(this);
            this.producerAgent.Subscribe<JointMessage>(this);
            this.producerAgent.Subscribe<SonarMessage>(this);
            this.producerAgent.Subscribe<TemperatureMessage>(this);

            this.model = new HeartbeatModel();
            if (this.maxUpdateIntervalMilliseconds == 0)
            {
                this.maxUpdateIntervalMilliseconds = DefaultUpdateInterval;
            }
        }

        /// <summary>
        /// Called to process subscription messages of a given type.
        /// </summary>
        /// <param name="state">The message to process.</param>
        public void Receive(GyroscopeMessage state)
        {
            double elapsed = Math.Max(state.Timestamp - this.lastTimestampInMilliseconds, 0.0001);
            this.model.SamplesPerSecond = ((double)(10000 * (state.SequenceId - this.lastSequenceId) / elapsed)) / 10;

            // let derived classes update the model data
            this.lastTimestampInMilliseconds = state.Timestamp;
            this.lastSequenceId = state.SequenceId;

            this.model.GyroXInRadiansPerSecond = state.State[0].RotationalVelocityInRadiansPerSecond.X;
            this.model.GyroYInRadiansPerSecond = state.State[0].RotationalVelocityInRadiansPerSecond.Y;
            this.model.GyroZInRadiansPerSecond = state.State[0].RotationalVelocityInRadiansPerSecond.Z;

            // send a notification to any the data bound controls about the model changes
            this.model.NotifyPropertyChanged();
        }

        /// <summary>
        /// Called to process subscription messages of a given type.
        /// </summary>
        /// <param name="state">The message to process.</param>
        public void Receive(AccelerometerMessage state)
        {
            double elapsed = Math.Max(state.Timestamp - this.lastTimestampInMilliseconds, 0.0001);
            this.model.SamplesPerSecond = ((double)(10000 * (state.SequenceId - this.lastSequenceId) / elapsed)) / 10;

            // let derived classes update the model data
            this.lastTimestampInMilliseconds = state.Timestamp;
            this.lastSequenceId = state.SequenceId;

            this.model.AccelForwardInG = state.State[0].AccelerationInG.X;
            this.model.AccelRightInG = state.State[0].AccelerationInG.Y;
            this.model.AccelUpInG = state.State[0].AccelerationInG.Z;

            // send a notification to any the data bound controls about the model changes
            this.model.NotifyPropertyChanged();
        }

        /// <summary>
        /// Called to process subscription messages of a given type.
        /// </summary>
        /// <param name="state">The message to process.</param>
        public void Receive(BatteryMessage state)
        {
            this.model.BatteryAvgTimeToEmptyInMinutes = -1;
            this.model.BatteryChargeInPercent = state.State.Count > 0 ? state.State[0].ChargePercentage : -1.0;
            this.model.BatteryCurrentInmA = state.State.Count > 0 ? state.State[0].CurrentInAmps : -1;
        }

        /// <summary>
        /// Called to process subscription messages of a given type.
        /// </summary>
        /// <param name="state">The message to process.</param>
        public void Receive(DifferentialDriveMessage state)
        {
            this.model.MotorLeftAccelerationInMperSec2 = state.State.Count > 0
                                                             ? state.State[0].LeftWheelAccelerationInMetersPerSecondSquared
                                                             : 0;
            this.model.MotorLeftPositionInMm = state.State.Count > 0 ? state.State[0].LeftWheelPositionInMeters : 0;
            this.model.MotorLeftVelocityInMperSec = state.State.Count > 0
                                                        ? state.State[0].LeftWheelVelocityInMetersPerSecond
                                                        : 0;

            this.model.MotorRightAccelerationInMperSec2 = state.State.Count > 0
                                                              ? state.State[0].RightWheelAccelerationInMetersPerSecondSquared
                                                              : 0;
            this.model.MotorRightPositionInMm = state.State.Count > 0 ? state.State[0].RightWheelPositionInMeters : 0;
            this.model.MotorRightVelocityInMperSec = state.State.Count > 0
                                                         ? state.State[0].RightWheelVelocityInMetersPerSecond
                                                         : 0;
        }

        /// <summary>
        /// Called to process subscription messages of a given type.
        /// </summary>
        /// <param name="state">The message to process.</param>
        public void Receive(FanMessage state)
        {
            this.model.LeftFanRpm = state.State.Count > 0 ? state.State[0].RotationsPerMinute : -1;
            this.model.RightFanRpm = state.State.Count > 1 ? state.State[1].RotationsPerMinute : -1;
        }

        /// <summary>
        /// Called to process subscription messages of a given type.
        /// </summary>
        /// <param name="state">The message to process.</param>
        public void Receive(HumidityMessage state)
        {
            this.model.RelativeHumidityInPercent = state.State.Count > 0
                                                       ? state.State[0].RelativeHumidityInPercentage
                                                       : 0;
        }

        /// <summary>
        /// Called to process subscription messages of a given type.
        /// </summary>
        /// <param name="state">The message to process.</param>
        public void Receive(IRProximityMessage state)
        {
            this.model.Prox1InMm = state.State.Count > 0 ? state.State[0].DistanceInMeters : -1;
            this.model.Prox2InMm = state.State.Count > 1 ? state.State[1].DistanceInMeters : -1;
            this.model.Prox3InMm = state.State.Count > 2 ? state.State[2].DistanceInMeters : -1;
            this.model.Prox4InMm = state.State.Count > 3 ? state.State[3].DistanceInMeters : -1;

            this.model.Cliff1InMm = state.State.Count > 4 ? state.State[4].DistanceInMeters : -1;
            this.model.Cliff2InMm = state.State.Count > 5 ? state.State[5].DistanceInMeters : -1;
            this.model.Cliff3InMm = state.State.Count > 6 ? state.State[6].DistanceInMeters : -1;
            this.model.Cliff4InMm = state.State.Count > 7 ? state.State[7].DistanceInMeters : -1;
            this.model.Cliff5InMm = state.State.Count > 8 ? state.State[8].DistanceInMeters : -1;
        }

        /// <summary>
        /// Called to process subscription messages of a given type.
        /// </summary>
        /// <param name="state">The message to process.</param>
        public void Receive(JointMessage state)
        {
            this.model.CameraHeadingInRadians = state.State.Count > 0 ? state.State[0].PositionInRadians : 0;
            this.model.TurretHeadingInRadians = state.State.Count > 1 ? state.State[1].PositionInRadians : 0;
        }

        /// <summary>
        /// Called to process subscription messages of a given type.
        /// </summary>
        /// <param name="state">The message to process.</param>
        public void Receive(SonarMessage state)
        {
            this.model.SonarDistanceLeftInMillimeters = state.State.Count > 0 ? state.State[0].DistanceInMeters : -1;
            this.model.SonarDistanceRightInMillimeters = state.State.Count > 1 ? state.State[1].DistanceInMeters : -1;
        }

        /// <summary>
        /// Called to process subscription messages of a given type.
        /// </summary>
        /// <param name="state">The message to process.</param>
        public void Receive(TemperatureMessage state)
        {
            this.model.BatteryTemperatureIn10ThsCelcius = state.State.Count > 0
                                                              ? (state.State[0].TemperatureInDegreesCelsius * 10)
                                                              : -1;
            this.model.TemperatureInternalIn10ThsCelsius = state.State.Count > 1
                                                               ? (state.State[1].TemperatureInDegreesCelsius * 10)
                                                               : -1;
            this.model.TemperatureExternalIn10ThsCelsius = state.State.Count > 2
                                                               ? (state.State[2].TemperatureInDegreesCelsius * 10)
                                                               : -1;
            this.model.TemperatureProjectorBlueInCelsius = state.State.Count > 3
                                                               ? (state.State[3].TemperatureInDegreesCelsius * 10)
                                                               : -1;
            this.model.TemperatureProjectorRedInCelsius = state.State.Count > 4
                                                              ? (state.State[4].TemperatureInDegreesCelsius * 10)
                                                              : -1;
            this.model.TemperatureProjectorGreenInCelsius = state.State.Count > 5
                                                                ? (state.State[5].TemperatureInDegreesCelsius * 10)
                                                                : -1;
        }

        /// <summary>
        /// Activates upstream agents.
        /// </summary>
        public override void OnActivated()
        {
            this.producerAgent.Activate();
        }

        /// <summary>
        /// Called by the UI to establish a link between the model agent and the view.
        /// The model agent calls the view, providing the model instance to which the view should data-bind.
        /// </summary>
        /// <param name="message">Control message containing a reference to the view.</param>
        public void ReceiveControl(ViewInitializationMessage message)
        {
            message.View.Dispatcher.Invoke(() => message.View.DataContext = this.model);
        }
    }
}
